#include "Services_QG.h"

extern uint8 u8scheduler_counter;
extern uint8 u8scheduler_flag;
extern uint8 u8sci_request;
extern uint8 u8accel_x;
extern uint8 u8accel_y;
extern uint8 u8accel_z;
extern uint8 u8speed;
extern uint8 u8battery;
extern uint8 u8buttons;
extern uint8 u8AccelStatus;

extern uint16 u16adc_x_10bit_max; 
extern uint16 u16adc_y_10bit_max;
extern uint16 u16adc_z_10bit_max;

extern uint16 u16adc_x_10bit_min; 
extern uint16 u16adc_y_10bit_min;
extern uint16 u16adc_z_10bit_min;

extern uint16 u16adc_x_10to8_offset;  
extern uint16 u16adc_y_10to8_offset;
extern uint16 u16adc_z_10to8_offset;

uint16 u16adc_new_x_offset;
uint16 u16adc_new_y_offset;
uint16 u16adc_new_z_offset;

extern uint8 u8recalibration_flag;

#define Page_Erase   PGM[21] = 0x40; u8temp = (( uint8 (*)( uint16 ))( PGM ))
#define Program_Byte PGM[21] = 0x20; u8temp = (( uint8 (*)( uint16, uint8 ))( PGM ))

/* Array of opcode instructions of the Erase/Program function in the HCS08 family */
volatile uint8 PGM[64]  =
{  
    0x87,0xC6,0x18,0x25,0xA5,0x10,0x27,0x08,0xC6,0x18,0x25,0xAA,0x10,0xC7,0x18,0x25,
    0x9E,0xE6,0x01,0xF7,0xA6,0x20,0xC7,0x18,0x26,0x45,0x18,0x25,0xF6,0xAA,0x80,0xF7,
    0x9D,0x9D,0x9D,0x9D,0x45,0x18,0x25,0xF6,0xF7,0xF6,0xA5,0x30,0x27,0x04,0xA6,0xFF,
    0x20,0x07,0xA6,0xFF,0xC7,0x18,0x00,0xC6,0x18,0x25,0xA5,0x40,0x27,0xF4,0x8A,0x81
};

#pragma MESSAGE DISABLE C1805
uint8 u8FlashErase (uint16 u16FlashPage)
{
    uint8 u8temp;
    uint8 u8Result;

    DisableInterrupts;
 
    u8temp = Page_Erase (u16FlashPage);

    if ( u8temp == 0xFF )
    {
        /* Signal Error */
        u8Result = FLASH_ERROR;
    }
    else
    {	
    	/* Signal Ok */
    	u8Result = FLASH_OK;
    }

 //   EnableInterrupts;

    return u8Result ;
}

uint8 u8FlashProgramByte (uint16 u16FlashPage, uint8 u8FlashByte)
{
    uint8 u8temp;
    uint8 u8Result = FLASH_OK;
    
    DisableInterrupts;
    
    u8temp = Program_Byte (u16FlashPage, u8FlashByte);
    
    if (u8temp == 0xFF)
    {
        /* Signal error */
        u8Result = FLASH_ERROR;
    }
    
    //EnableInterrupts;
    
    return u8Result;
}

void vfnFlashConfigureClock (void)
{
    FCDIV_DIV = 39; // Value for a 200 khz flash frequency from an 8 MHz bus
}

void vfnFlashProtect (void)
{
    // Last unprotected flash address is 0xE1FF and protection is activated
    FPROT = 0xE0;     
}


void vfnICS_Configure (void)
{
    ICSC1 = ICS_CONTROL_REG1;
    ICSC2 = ICS_CONTROL_REG2;
 //   ICSTRM = NVFTRIM;                   
  //  ICSSC_FTRIM = NVFTRIM_FTRIM;
    ICSSC_FTRIM = 0x01;
    ICSTRM = 0xAC;
    
          
}

void vfnSystemOptionsInit (void)
{
    SOPT1 = SYS_CONFIG_1;
    SOPT2 = SYS_CONFIG_2;      
}

void vfnSystemTimeBaseSetup (void)
{
    vfnMTIM_ClockSelect (MTIM_BUS_CLOCK); // Select bus clock (8 MHz)
    vfnMTIM_PrescalerSelect (MTIM_PRESCALER_256);
    vfnMTIM_SetModulo (250); // Time base is 8ms
    vfnMTIM_InterruptEnable (MTIM_INTERRUPTS_ENABLED);
    vfnMTIM_SetStatus (MTIM_ACTIVE);     
}

#pragma INLINE
void vfnSystemWait (void)
{
    asm
    {
        WAIT;
    }
}

interrupt 12 void isrTimeBase (void)
{
    (void)u8MTIM_CheckOvFlag ();
    vfnMTIM_ClearOvFlag ();
    
    u8scheduler_counter++;
    
    if (u8scheduler_counter & 0x01)
    {
        u8scheduler_flag = TASK_BUTTONS;
    }
    else
    {
        u8scheduler_flag = TASK_SENSORS;   
    }
       
}

interrupt 15 void isrSCI_Rx (void)
{
    //uint8 u8tempbuffer;
    uint8 u8cksum = 0;
    (void) u8SCI_RxPending();
    u8sci_request = u8SCI_ReadByte();
    
    if (u8sci_request == NORMAL_PACKET_REQUEST)
    {
        // If the requested packet is a normal data packet
        // Send all the sensor data compiled by the auxiliary MCU
        vfnSCI_SendByte (u8accel_x);
        u8cksum ^= u8accel_x;
        while(!u8SCI_TxBufferEmpty());
        
        vfnSCI_SendByte (u8accel_y);
        u8cksum ^= u8accel_y;
        while(!u8SCI_TxBufferEmpty());
        
        vfnSCI_SendByte (u8accel_z);
        u8cksum ^= u8accel_z;
        while(!u8SCI_TxBufferEmpty());
        
        vfnSCI_SendByte (u8speed);
        u8cksum ^= u8speed;
        while(!u8SCI_TxBufferEmpty()); 
        
        vfnSCI_SendByte (u8battery);
        u8cksum ^= u8battery;
        while(!u8SCI_TxBufferEmpty());
        
        vfnSCI_SendByte (u8buttons);
        u8cksum ^= u8buttons;
        u8buttons &= 0xF0;    //Last pressed buttons are erased until next serial TX
                              //Current pressed buttons are erased by main
        while(!u8SCI_TxBufferEmpty());
        
        vfnSCI_SendByte (u8cksum);
        while(!u8SCI_TxBufferEmpty());
        
                 
    }
    else if (u8sci_request == ADC_CALIBRATE_REQUEST)
    {
        
        vfnADC_ChannelSelect (SENSOR_ACCEL_X_CH);
        while(!u8ADC_CheckCCF());
        
        u16adc_new_x_offset = u16ADC_GetResult();
 
        vfnADC_ChannelSelect (SENSOR_ACCEL_Y_CH);
        while(!u8ADC_CheckCCF());
        
        u16adc_new_y_offset = u16ADC_GetResult();
        
        vfnADC_ChannelSelect (SENSOR_ACCEL_Z_CH);
        while(!u8ADC_CheckCCF());
        
        u16adc_new_z_offset = u16ADC_GetResult() - SENSOR_ACCEL_1G;
                
        u8recalibration_flag = RECALIBRATION_REQUESTED;        
       
    }
    else if (u8sci_request == HELLO_PACKET)
    {
        if(u8AccelStatus == SENSOR_ACCEL_TEST_OK)
        {
        	vfnSCI_SendByte (HELLO_OK);
        }
        else
        {
        	vfnSCI_SendByte (HELLO_ACCEL_FAIL);
        }
        while(!u8SCI_TxBufferEmpty());    
    }
    else
    {
        // Packet Request Error
        // Not answer. Xbee will go timeout.
    }
    
    
}